package mirabilis.org.util;

import java.security.Signature;
import java.util.Iterator;

import mirabilis.org.config.Configuration;
import mirabilis.org.simplices.Coordinates;
import mirabilis.org.simplices.Simplex;
import mirabilis.org.simplices.Vertex;

import org.jblas.Decompose;
import org.jblas.DoubleMatrix;
import org.jblas.Solve;

public class Gradients {

	public static double[] unit_gradient(Simplex simplex){
		
		// We calculate the unit vector of the gradient dividing by the norm (magnitude of the vector)
		double[] grad = gradient(simplex);
		
		double sum = 0;
		for(int i=0;i<grad.length;i++){
			sum += Math.pow(grad[i], 2);
		}
		
		double magnitude =	Math.sqrt(sum);
		double[] unit = new double[grad.length];
		
		for(int i=0;i<grad.length;i++){
			unit[i] = grad[i] / magnitude;
		}
		
		return unit;
	}
	
	public static double[] unit_gradient_stabilizer(Simplex simplex,double[] bow_coef){
		
		// We calculate the unit vector of the gradient dividing by the norm (magnitude of the vector)
		double[] grad = gradient_hyperplane(simplex, bow_coef);
		
		double sum = 0;
		for(int i=0;i<grad.length;i++){
			sum += Math.pow(grad[i], 2);
		}
		
		double magnitude =	Math.sqrt(sum);
		double[] unit = new double[grad.length];
		
		for(int i=0;i<grad.length;i++){
			unit[i] = grad[i] / magnitude;
		}
		
		return unit;
	}
	
	public static double[] gradient(Simplex simplex){
		
		
		//------------------------
		// Here we create the ST and delta Matrices
		//------------------------
		
		//We are gonna calculate the gradient using the arrow as reference (gradient is uniform in the simplex)
		Vertex arrow = simplex.getArrow();
		
		DoubleMatrix arrowJBLAS = new DoubleMatrix(arrow.getCoordinates().getCoordinatesAsDouble());
		DoubleMatrix _ST = new DoubleMatrix(Configuration.N,Configuration.N);
		DoubleMatrix _delta = new DoubleMatrix(Configuration.N,1);
		
		int index = 0;
		for (Iterator<Vertex> it = simplex.iterator(); it.hasNext();){
			//For each vertex in the remaining of the simplex (mind that the arrow has been already removed)
			Vertex y_n = it.next();
			if (!it.hasNext())// the last one is the arrow and we do not compute it
				break;
			
			//Compute ST where S is [y1-y0,....,yn-y0] 
			DoubleMatrix y_nJBLAS = new DoubleMatrix(y_n.getCoordinates().getCoordinatesAsDouble());
			y_nJBLAS.subi(arrowJBLAS);
			_ST.putRow(index, y_nJBLAS);
			
			//Compute delta as [f(y1)-f(y0),....,f(yn)-f(y0)]T
			//TODO: Try to clean up next lines to something more elegant
			double fyn=0,fyo=0;
			try {fyn = y_n.getFitness();fyo = arrow.getFitness(); }catch (Exception e){	e.printStackTrace();}
			double[] aux={fyn-fyo};
			DoubleMatrix diff = new DoubleMatrix(aux);
			_delta.putRow(index, diff);
			
			index++;
	    }
		
		//------------------------
		// Here we solve the gradient (nabla)
		//------------------------
		
		DoubleMatrix _nabla = null;
		try {
			_nabla = Solve.solve(_ST, _delta);	
		} catch (Exception e) {
			// TODO: handle exception
			e.printStackTrace();
			return solvingSingularity(simplex);
		}
		
		
//		double [] grad = new double[Configuration.N];
//		for (int i=0;i<Configuration.N;i++){
//			grad[i] = _nabla.get(i);
//		}
							
		return _nabla.toArray();
	}
	
	public static double[] normalize_vector(double[] v){
		double sum = 0;
		for(int i=0;i<v.length;i++){
			sum += Math.pow(v[i], 2);
		}
		
		double magnitude =	Math.sqrt(sum);
		double[] unit = new double[v.length];
		
		for(int i=0;i<v.length;i++){
			unit[i] = v[i] / magnitude;
		}
		
		return unit;
	}
	
	public static double[] gradient_hyperplane(Simplex simplex,double[] bow_coef){

		//------------------------
		// Here we create the ST and delta Matrices
		//------------------------
				
		//We are gonna calculate the gradient using the stabilizer as reference (gradient is uniform in the simplex)
		Vertex stabilizer = simplex.getBest();
		DoubleMatrix yoJBLAS = new DoubleMatrix(stabilizer.getCoordinates().getCoordinatesAsDouble());
		double[] norm = new double[bow_coef.length-1];
		System.arraycopy(bow_coef, 0, norm, 0, bow_coef.length-1);
		norm = normalize_vector(norm);
		
		DoubleMatrix normJBLAS = new DoubleMatrix(norm);
		yoJBLAS.addi(normJBLAS);

		double fyo = 0;
		try {fyo = stabilizer.getFitness();	} catch (Exception e) {	e.printStackTrace();}
		
		DoubleMatrix _ST = new DoubleMatrix(Configuration.N,Configuration.N);
		DoubleMatrix _delta = new DoubleMatrix(Configuration.N,1);
		
		int index = 0;
		for (Iterator<Vertex> it = simplex.iterator(); it.hasNext();){
			Vertex y_n = it.next();
			
			//Compute ST where S is [y1-y0,....,yn-y0] 
			DoubleMatrix y_nJBLAS = new DoubleMatrix(y_n.getCoordinates().getCoordinatesAsDouble());
			y_nJBLAS.subi(yoJBLAS);
			_ST.putRow(index, y_nJBLAS);
			
			//Compute delta as [f(y1)-f(y0),....,f(yn)-f(y0)]T
			//TODO: Try to clean up next lines to something more elegant
			double fyn=0;
			try {fyn = y_n.getFitness(); }catch (Exception e){	e.printStackTrace();}
			double[] aux={fyn-fyo};
			DoubleMatrix diff = new DoubleMatrix(aux);
			_delta.putRow(index, diff);
			
			index++;
	    }
		
		//------------------------
		// Here we solve the gradient (nabla)
		//------------------------
		
		DoubleMatrix _nabla = null;
		try {
			_nabla = Solve.solve(_ST, _delta);	
		} catch (Exception e) {
			// TODO: handle exception
			e.printStackTrace();
			System.exit(-1);
			return solvingSingularity(simplex);
		}
		
		
//		double [] grad = new double[Configuration.N];
//		for (int i=0;i<Configuration.N;i++){
//			grad[i] = _nabla.get(i);
//		}
							
		return _nabla.toArray();
		
		
	}
	
	public static double[] getAim(double[] best,double[] worst){
		double[] middle = new double[best.length];
		
		for(int i=0;i<best.length;i++){
			middle[i] = (best[i]+worst[i]) / 2.0;
		}
		
		return middle;
	}
	
	// Returns a point that is symmetric to the @param point with respect to the @param hyperplane
	public static double[] getSymmetric(double[] hyperplane, double[] point){
		
		int n = point.length;
		DoubleMatrix A = DoubleMatrix.zeros(n, n);
		DoubleMatrix B = DoubleMatrix.zeros(n, 1);

		if(hyperplane[0] == 0){
			A.put(0, 0, 1.0);
			B.put(0, 0, point[0]);
			for(int i=2;i<n;i++){
				A.put(i-1, 1, 1.0/hyperplane[1]);
				A.put(i-1, i,-1.0/hyperplane[i]);
				double value = point[1]/(hyperplane[1]) - point[i]/(hyperplane[i]);
				B.put(i-1, value);
			}
			
		}else{
			for(int i=1;i<n;i++){
				if(hyperplane[i]==0){
					A.put(i-1, i, 1.0);
					B.put(i-1, point[i]);
				}else{
					A.put(i-1, 0, 1.0/hyperplane[0]);
					A.put(i-1, i,-1.0/hyperplane[i]);
					double value = point[0]/(hyperplane[0]) - point[i]/(hyperplane[i]);
					B.put(i-1, value);	
				}
			}
		}
		
		
		//Add the hyperplane in the last row
		
		double[] hyperplane_without_D = new double[n];
		System.arraycopy(hyperplane, 0, hyperplane_without_D, 0, n);
		A.putRow(n-1, new DoubleMatrix(hyperplane_without_D));		
		B.put(n-1, 0, -1.0 * hyperplane[n]);
		
		//System.out.println(Bounds.doubleArrayAsString(hyperplane));
		//A.print();
		//B.print();
		
		double[] m_point = Solve.solve(A, B).toArray();
				
		//System.out.println("mpoint "+Bounds.doubleArrayAsString(m_point));
				
		double[] symmetric_point = new double[n];
				
		for (int i=0;i<n;i++){
			symmetric_point[i] = 2 * m_point[i] - point[i];
		}
				
		return symmetric_point;
	}

	
	//Hack to solve not singular matrices due to numerical precision problems
	// The trick is to return a vector which is orthogonal to the bow
	public static double[] solvingSingularity(Simplex simplex){
		//working copy of the simplex
		Simplex wcsimplex = (Simplex)simplex.clone();
		
		wcsimplex.pollArrow();
		
		double[] bow = getBowHyperplane(wcsimplex);
		//double[] parallelhyperplane = new double[Configuration.N+1];
		//System.arraycopy(bow, 0, parallelhyperplane, 0, Configuration.N);
		
		//parallelhyperplane[Configuration.N] = bow[Configuration.N]+1; // The +1 at the D coefficient is the trick to get a parallel hyperplane at +1 distance
		
		// Calculate centroid
		double[] centroid = DoubleMatrix.zeros(Configuration.N).toArray();
		for(Iterator<Vertex> it= wcsimplex.iterator();it.hasNext();){
			Vertex v = it.next();
			double[] coord = v.getCoordinates().getCoordinatesAsDouble();
			for(int i=0;i<coord.length;i++){
				centroid[i] += coord[i]/(1.0*Configuration.N);//Mind that N here does not refer to the dimensions but to the number of vertices in the bow
			}
		}
		
		double[] pointA = DoubleMatrix.zeros(Configuration.N).toArray();
		double[] pointB = DoubleMatrix.zeros(Configuration.N).toArray();
		
		 
		for(int i=0;i<pointA.length;i++){
			double K = 0.0; //Any real numbuer
			pointA[i] = K * bow[i] + centroid[i]; //This will be actually the centroid
			K = 1.0;
			pointB[i] = K * bow[i] + centroid[i];
		}
		
		double[] gradient = DoubleMatrix.zeros(Configuration.N).toArray();
		for(int i=0;i<pointA.length;i++){
			gradient[i] = pointB[i] - pointA[i];
		}
		
		return gradient;
	}
	
	//Solution to the hyperplane "BOW"
	//Precondition: the simplex should only contain coordinates of the hyperplane "Bow"
	//              in other words, the function simplex.pollArrow() should have been previously called.
	public static double[] getBowHyperplane(Simplex simplex){
		//double[][] _bow = new double[Configuration.N][Configuration.N+1];
		DoubleMatrix _bow = new DoubleMatrix(Configuration.N,Configuration.N);
		
		
		// Building a Matrix
		//	  (x-xo) u1 v1
		//    (y-yo) u2 v2
		//    (z-zo) u3 v3
		int index=0;
		Iterator<Vertex> it = simplex.iterator();
		Vertex vertex = it.next();
		DoubleMatrix point = new DoubleMatrix(vertex.getCoordinates().getCoordinatesAsDouble());
		
		DoubleMatrix ones = DoubleMatrix.ones(Configuration.N,1);
		_bow.putColumn(index, ones);
		index++;
		
		for (; it.hasNext();){
			Vertex y_n = it.next();
			//Compute ST where S is [y1-y0,....,yn-y0] 
			DoubleMatrix y_nJBLAS = new DoubleMatrix(y_n.getCoordinates().getCoordinatesAsDouble());
			y_nJBLAS.subi(point);
			_bow.putColumn(index, y_nJBLAS);
			
			//_bow[index][Configuration.N]=-1;
			index++;
		}
		
	
		//Get the cofactors for the first column
		// This values are the A, B, C of the implicit equation of the hyperplane
		// Ax + By + Cz + D = 0
		DoubleMatrix implicit = cofactorBow(_bow);
		double[] coefficients = new double[Configuration.N+1];
		System.arraycopy(implicit.toArray(), 0, coefficients, 0, Configuration.N);
		
		//Normalization of the coefficients
//		double norm=0;
//		for(int i=0;i<Configuration.N;i++){
//			norm += Math.pow(coefficients[i], 2);
//		}
//		norm = Math.sqrt(norm);
//		
//		for(int i=0;i<Configuration.N;i++){
//			coefficients[i] /= norm;
//		}
		
		//D = -Axo -Byo -Czo
		double D = 0;
		for(int i=0;i<Configuration.N;i++){
			D -= coefficients[i] * point.get(i);
		}
		coefficients[Configuration.N] = D;
			
		//This is made to avoid -0.0 in coefficients
		for(int i=0;i<coefficients.length;i++)
			coefficients[i] = coefficients[i] + 0.0;
		
		return coefficients;//
		//return lsolve(_bow, zeros.toArray());
				
	}
	
	//The "Rest of the Arrow" is the intersecting point determined by the "arrow+gradient" vector and the bow plane
	public static double[] getRestArrow(double[] gradients, double[] arrow, double[] bow){
		return getIntersectionOfaVectorWithHyperplane(gradients, arrow, bow);
		
	}
	
	public static double[] getAim(Simplex onlybow, double[] arrow, double target[]){
		// Calculate centroid
			double[] centroid = centroid(onlybow);
		
		//Bisector from arrow to centroid		
		double[] bisector = DoubleMatrix.zeros(Configuration.N).toArray();
		for(int i=0;i<bisector.length;i++){
			bisector[i] = arrow[i]-centroid[i];
		}
		
		return getIntersectionOfaVectorWithHyperplane(bisector, arrow, target);
				
	}
	
	public static double[] centroid(Simplex simplex){
		int n = simplex.size();
		double[] centroid = DoubleMatrix.zeros(Configuration.N).toArray();
		for(Iterator<Vertex> it= simplex.iterator();it.hasNext();){
			Vertex v = it.next();
			double[] coord = v.getCoordinates().getCoordinatesAsDouble();
			for(int i=0;i<coord.length;i++){
				centroid[i] += coord[i]/(1.0*n);//Mind that n here does not refer to the dimensions but to the number of vertices in the bow
			}
		}
		return centroid;
	}
	
	// This method is essentially the same as getRestArrow. The only different is that the
	// the sought point is at the target hyperplane
	// Therefore, this function is a wrapper on getRestArrow
	public static synchronized double[] getIntersectionOfaVectorWithHyperplane2(double[] vector, double[] head, double[] hyperplane){
		
		//Construct the implicit equations of the line 
				//As a result we get a line defined
				DoubleMatrix A = DoubleMatrix.zeros(Configuration.N, Configuration.N);
				DoubleMatrix B = DoubleMatrix.zeros(Configuration.N, 1);
				
				for(int i=1;i<Configuration.N;i++){
					A.put(i-1, 0, signGradient()*vector[i]);
					A.put(i-1, i,-signGradient()*vector[0]);
					double value =signGradient()*vector[i]*head[0]-signGradient()*vector[0]*head[i];
					B.put(i-1, 0, value);
				}
				
				//Add the plane with the target to compute the intersection of the "arrow" line with the target
				
				double[] bow_minus_D = new double[Configuration.N];
				System.arraycopy(hyperplane, 0, bow_minus_D, 0, Configuration.N);
				A.putRow(Configuration.N-1, new DoubleMatrix(bow_minus_D));
				
				B.put(Configuration.N-1, 0, -1.0 * hyperplane[Configuration.N]);
				try {
					return Solve.solve(A, B).toArray();	
				} catch (Exception e) { // If the system does not have a solution
					e.printStackTrace();
					return null;
				}
				
	}
	
	public static synchronized double[] getIntersectionOfaVectorWithHyperplane(double[] vector, double[] head, double[] hyperplane){
		
		int sizeofmatrix = Configuration.N;
		int first_nonfixed_position=-1;
		double[] returnpoint = new double[Configuration.N];
		boolean[] fixed_position = new boolean[Configuration.N];
		
		for(int i=0;i<Configuration.N;i++){
			if(vector[i]==0){
				fixed_position[i] = true;
				returnpoint[i] = head[i];
				sizeofmatrix--;
			}else if(first_nonfixed_position==-1){
				first_nonfixed_position = i;
			}
		}
		
		//System.err.println("sizeofmatrix: "+sizeofmatrix);
		
		//if the size of the resulting matrix is 1, we solve directly using the hyperplane
		if(sizeofmatrix == 1){
			double value = -1.0*hyperplane[Configuration.N];
			for(int i=0;i<Configuration.N;i++){
				if(i!=first_nonfixed_position){
					value -= returnpoint[i]*hyperplane[i];
				}
			}
			returnpoint[first_nonfixed_position] = value/hyperplane[first_nonfixed_position];
			return returnpoint;
		}
		
		//in case that the matrix is larger
		
		//Construct the implicit equations of the line 
		//As a result we get a line defined
		DoubleMatrix A = DoubleMatrix.zeros(sizeofmatrix, sizeofmatrix);
		DoubleMatrix B = DoubleMatrix.zeros(sizeofmatrix, 1);
		
		int index = first_nonfixed_position;
		
		for(int i=1;i<sizeofmatrix;i++){
			double vectori=1,headi=1;
			for(int j=index+1;j<Configuration.N;j++){
				if(!fixed_position[j]){
					index = j;
					vectori = vector[j];
					headi = head[j];
					break;
				}
			}
			
			A.put(i-1, 0, vectori);
			A.put(i-1, i,-1.0*vector[first_nonfixed_position]);
			double value = vectori*head[first_nonfixed_position]-vector[first_nonfixed_position]*headi;
			B.put(i-1, 0, value);
		}
		
		
		//Add the plane with the target to compute the intersection of the "arrow" line with the target
	
		index = 0;
		double[] bow_minus_D = new double[sizeofmatrix]; 
		for(int i=0;i<Configuration.N;i++){
			if(!fixed_position[i]){
				bow_minus_D[index] = hyperplane[i];
				index++;
			}
		}
		A.putRow(sizeofmatrix-1, new DoubleMatrix(bow_minus_D));
		
		double value = -1.0*hyperplane[Configuration.N];
		for(int i=0;i<Configuration.N;i++){
			if(fixed_position[i]){
				value -= returnpoint[i]*hyperplane[i];
			}
		}
		B.put(sizeofmatrix-1, 0, value);
		
		//Solving the matrix
		double[] solution=null;
		try {
			solution = Solve.solve(A, B).toArray();	
		} catch (Exception e) { // If the system does not have a solution
			//e.printStackTrace();
			return null;
		}
		
		//Reconstructing the point
		index = 0;
		for(int i=0;i<Configuration.N;i++){
			if(!fixed_position[i]){
				returnpoint[i] = solution[index];
				index++;
			}
		}
		
		return returnpoint;
	}

		
	
		
	
	
	private static double signGradient(){
		if (Configuration.minimization)
			return -1.0;
		else
			return 1.0;
	}
	
		
	//Get both target hyperplanes
	// The inner target is at position 0
	// The outside target is at position 1
	public static double[][] getTargets(double distance,double[] arrow, double[] rest, double[] bowcoef){
		
		// Direction coefficients for the "targets" are equal to those of the "bow", i.e. they are parallel hyperplanes
		double[][] targets = new double[2][Configuration.N+1];
		System.arraycopy(bowcoef, 0, targets[0], 0, Configuration.N);
		System.arraycopy(bowcoef, 0, targets[1], 0, Configuration.N);
		
//We need to calculate the remaining coefficient, i.e. D in Ax+By+Cz+D=0
// Since the bow has two parallel hyperplanes at the same distance from the bow...
// ... the two different Ds stand for the inner and outside target

		
		//Calculating D for the hyperplane, parallel to the bow, that contains the arrow
		double Darrow = 0;
		for(int i=0;i<Configuration.N;i++){
			Darrow -= bowcoef[i] * arrow[i]; 
		}
		
		//Calculating the Ds for the two hyperplanes at the same distance of the bow
		// distance = |Axo + Byo + Czo + D| / sqrt(A^2 + B^2 + C^2)
		// where xo,yo,zo is the rest point at the bow
		
		// sqrt(A^2 + B^2 + C^2)
		double sqrt=0;
		for(int i=0;i<Configuration.N;i++){
			sqrt += Math.pow(bowcoef[i],2) ; 
		}
		sqrt = Math.sqrt(sqrt);
		
		// distance * sqrt(A^2 + B^2 + C^2)
		double distanceXsqrt = distance * sqrt;
		
		//  Axo + Byo + Czo is the rest point
		double aixi = 0;
		for(int i=0;i<Configuration.N;i++){
			aixi -= bowcoef[i] * rest[i] ; 
			//System.err.print(rest[i]+",");
		}
		//System.err.println();
		
		// D = Axo + Byo + Czo (+-) distance * sqrt(A^2 + B^2 + C^2)
		double D1 = aixi + distanceXsqrt;
		double D2 = aixi - distanceXsqrt;
		
		//System.err.println("aixi: "+aixi+" distSQRT: "+distanceXsqrt+" D1: "+D1+" D2: "+D2);
		
		// Now we have, 4 parallel hyperplanes all with the same coefficient A B C
		// - The bow with D
		// - The parallel hyperplane passing by the arrow with Darrow
		// - Two parallel hyperplanes to the bow at the given distance from the bow
		
		//We have to order the target hyperplanes, the inner (0) is the closer to the arrow
		// while the outer (1) is the furthest from the arrow
		if(Darrow > bowcoef[Configuration.N]){
			if(D1 < bowcoef[Configuration.N]){
				targets[0][Configuration.N] = D2;
				targets[1][Configuration.N] = D1;
			}else{
				targets[0][Configuration.N] = D1;
				targets[1][Configuration.N] = D2;
			}
		}else{
			if(D1 > bowcoef[Configuration.N]){
				targets[0][Configuration.N] = D2;
				targets[1][Configuration.N] = D1;
			}else{
				targets[0][Configuration.N] = D1;
				targets[1][Configuration.N] = D2;
			}
		}
				
		return targets;
	}
	
	//We calculate the distance in which the target has to be placed from the bow
	//If V is the volume of the simplex, we may define e.g. V' = V / 2
	//In general V = h A/n, so that the targeted new volume V' can be defined as V' = h' A/n
	//Here we want to calculate this new h'. So we solve:
	// h' = V'n / A
	public static double getHeightFromBowToTarget(double VolumeSimplex,double targetedNewVolume, double[] arrow, double[] bow){

		double hyperarea = getHyperareaBow(VolumeSimplex, arrow, bow);
		return (targetedNewVolume * Configuration.N) / hyperarea;
	}

	// In general V = h A/n
	// where V is hypervolume, h height, A hyperarea and n dimensions
	// We can solve A and have: A = Vn / h
	private static double getHyperareaBow(double VolumeSimplex,double[] arrow, double[] bow){
		double height = getHeightArrowToBow(arrow, bow);
		return (VolumeSimplex * Configuration.N) / height;
	}
	
	//This is the height of the arrow to the bow expressed as the distance:
	// distance = |Axo + Byo + Czo + D| / sqrt(A^2 + B^2 + C^2)
	// where xo,yo,zo is the arrow point and A B C D are the coefficients of the bow	
	private static double getHeightArrowToBow( double[] arrow, double[] bow){
		
		double axo = 0;
		double sqrt = 0;
		for(int i=0;i<Configuration.N;i++){
			axo += arrow[i] * bow[i];
			sqrt += Math.pow(bow[i], 2);
		}
		axo += bow[Configuration.N]; // This is the D component
		axo = Math.abs(axo);
		sqrt = Math.sqrt(sqrt);
		
		return axo/sqrt;
	}
	
	//It returns de cofactors of the first column in:
	//    (x-xo) u1 v1
	//    (y-yo) u2 v2
	//    (z-zo) u3 v3
 	private static DoubleMatrix cofactorBow(DoubleMatrix matrix){
		 DoubleMatrix mat = new DoubleMatrix(matrix.rows, 1);
		    
		    for (int i=0;i<matrix.rows;i++) {
		            try {
						mat.put(i, 0, changeSign(i,0) * lupdeterminant(createSubMatrix(matrix, i, 0)));
					} catch (Exception e) {
						e.printStackTrace();
					}
		    }
		    
		    return mat;
	}
	
	public static DoubleMatrix cofactor(DoubleMatrix matrix){
	    DoubleMatrix mat = new DoubleMatrix(matrix.rows, matrix.columns);
	    
	    for (int i=0;i<matrix.rows;i++) {
	        for (int j=0; j<matrix.columns;j++) {
	            try {
					mat.put(i, j, changeSign(i,j) * lupdeterminant(createSubMatrix(matrix, i, j)));
				} catch (Exception e) {
					e.printStackTrace();
				}
	        }
	    }
	    
	    return mat;
	}
	
	public static double lupdeterminant(DoubleMatrix matrix) throws Exception{
		 if (!matrix.isSquare())
		        throw new Exception("matrix need to be square.");
		
		Decompose.LUDecomposition<DoubleMatrix> X = Decompose.lu(matrix);
		
		int sign = sign(X.p);
		double ldeterminant = 1.0;
		double udeterminant = 1.0;
		
		for(int i=0;i<X.u.columns;i++){
			udeterminant *= X.u.get(i, i);
		}

		for(int i=0;i<X.l.columns;i++){
			ldeterminant *= X.l.get(i, i);
		}

		return ldeterminant * udeterminant * sign;
		
	}

	// ----------------------------------------------------------
	// Calculates the sign of a permutation p.
	// p is a row vector p(1,n), which represents the permutation.
	// sgn(p) = (-1)^(No. of even-length cycles)
	// Complexity : O(n + ncyc) ~ O(n + Hn) ~~ O(n+log(n)) steps.
	//
	// Derek O'Connor 20 March 2011.
	// Java version by Juan Luis Jimenez
	// ----------------------------------------------------------
	private static int sign(DoubleMatrix p){
		int[] p_vector = p_vector(p);
		
		int n   = p_vector.length;
		boolean [] visited = new boolean[n];	// Logical vector which marks all p(k)
		                                       // not visited.
		int sgn = 1;
		for (int k=0;k<n;k++){
			if (!visited[k]){					// k not visited, start of new cycle
				int next = k;
				int L = 0;
				while (!visited[next]){			// Traverse the current cycle k
					L = L+1;					// and find its length L
					visited[next] = true;
					next = p_vector[next];
				}
				if (L%2 == 0){   	           // If L is even, change sign.
					sgn = -sgn;
				}
			}
		}
		return sgn;
	}
	
	private static int[] p_vector(DoubleMatrix p){
		int[] p_vector = new int[p.columns];
		for(int i=0;i<p.rows;i++){
			for(int j=0;j<p.columns;j++){
				if(p.get(i, j) != 0){
					p_vector[i] = j;
					break;
				}
			}
		}
		
		return p_vector;
	}
	

	
	public static double determinant(DoubleMatrix matrix) throws Exception{
	    if (!matrix.isSquare())
	        throw new Exception("matrix need to be square.");
	    
	    if (matrix.rows==2) {
	        return (matrix.get(0, 0) * matrix.get(1, 1)) - ( matrix.get(0, 1) * matrix.get(1, 0));
	    }
	    
	    if (matrix.rows==1) {//for the cofactor of a matrix of dimension 2
	        return (matrix.get(0, 0));
	    }
	    
	    double sum = 0.0;
	    for (int i=0; i<matrix.columns; i++) {
	        sum += changeSign(0,i) * matrix.get(0, i) * determinant(createSubMatrix(matrix, 0, i));
	    }
	    return sum;
	}


	
	private static DoubleMatrix createSubMatrix(DoubleMatrix matrix, int excluding_row, int excluding_col) {
	    DoubleMatrix mat = new DoubleMatrix(matrix.rows-1, matrix.columns-1);
	    int r = -1;
	    for (int i=0;i<matrix.rows;i++) {
	        if (i==excluding_row)
	            continue;
	            r++;
	            int c = -1;
	        for (int j=0;j<matrix.columns;j++) {
	            if (j==excluding_col)
	                continue;
	            mat.put(r, ++c, matrix.get(i, j));
	        }
	    }
	    return mat;
	}
	


	private static double changeSign(int row,int column){
		return Math.pow(-1, (row+column));
	}






	
	
	// Implementation by  Robert Sedgewick and Kevin Wayne
    // Gaussian elimination with partial pivoting
    public static double[] lsolve(double[][] A, double[] b) {
        int N  = b.length;
        double EPSILON = 1e-10;
        
        for (int p = 0; p < N; p++) {

            // find pivot row and swap
            int max = p;
            for (int i = p + 1; i < N; i++) {
                if (Math.abs(A[i][p]) > Math.abs(A[max][p])) {
                    max = i;
                }
            }
            double[] temp = A[p]; A[p] = A[max]; A[max] = temp;
            double   t    = b[p]; b[p] = b[max]; b[max] = t;

            // singular or nearly singular
            if (Math.abs(A[p][p]) <= EPSILON) {
                throw new RuntimeException("Matrix is singular or nearly singular");
            }

            // pivot within A and b
            for (int i = p + 1; i < N; i++) {
                double alpha = A[i][p] / A[p][p];
                b[i] -= alpha * b[p];
                for (int j = p; j < N; j++) {
                    A[i][j] -= alpha * A[p][j];
                }
            }
        }

        // back substitution
        double[] x = new double[N];
        for (int i = N - 1; i >= 0; i--) {
            double sum = 0.0;
            for (int j = i + 1; j < N; j++) {
                sum += A[i][j] * x[j];
            }
            x[i] = (b[i] - sum) / A[i][i];
        }
        return x;
    }

	// A little hack to go:
	//    -from a Simplex with N+1 vertices of dimension N
	//    -to a single long vector containing all vertices
    public static double[] hackToGoArrayContainingASimplex(Simplex simplex){
    	
    	double[] simplexInArray = new double[Configuration.N * (Configuration.N + 1)];
    	
    	int index = 0;
    	for (Iterator<Vertex> it = simplex.iterator(); it.hasNext();){
			Vertex y_n = it.next();
			System.arraycopy(y_n.getCoordinates().getCoordinatesAsDouble(), 0, simplexInArray, (index * Configuration.N), Configuration.N);
			index++;
    	}
    	return simplexInArray;
    }
    

	
}
